/**
 * 该例程监听tf数据，并计算、发布turtle2的速度指令 为了测试使用
 * 运行流程：
 *         roscore
 *         rosrun turtlesim turtlesim_node
 *         rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1
 *         rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2
 *         rosrun learning_tf turtle_tf_listener
 *         rosrun turtlesim turtle_teleop_key
 */

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

int main(int argc, char** argv){
	ros::init(argc, argv, "my_tf_listener");
	ros::NodeHandle node;
	
	ros::service::waitForService("/spawn");// 请求产生turtle2
	ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
	turtlesim::Spawn srv;
	add_turtle.call(srv);

	ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);

	tf::TransformListener listener;

	ros::Rate rate(10.0);
	while (node.ok()){
		// 获取turtle1与turtle2坐标系之间的tf数据
		tf::StampedTransform transform;
		try{
			listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
			listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
		}
		catch (tf::TransformException &ex){
			ROS_ERROR("%s",ex.what());
			ros::Duration(1.0).sleep();
			continue;
		}

		// 根据turtle1与turtle2坐标系之间的位置关系，发布turtle2的速度控制指令
		geometry_msgs::Twist vel_msg;
		vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x());
		vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2)); //C 库函数 double pow(double x, double y) 返回 x 的 y 次幂
		turtle_vel.publish(vel_msg);

		rate.sleep();
	}
	return 0;
};
